#include "../DWMRICore/Fiber.h"
#include "../DWMRICore/Point3I.h"

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include <vector>

using namespace std;

char *fiber_pathname = NULL;
char *dst_pathname = NULL;

/* for debug, the output voxel */
bool debug_mode_flag = false;
CPoint3I pos;

void PrintUsage()
{
	printf("Usage: MappingFibers -fiber configure_file -dst dst_file\n");
}

int ParseArguments(const int argc, char *argv[])
{
	for (int i = 1; i < argc; ++i) {
		if (strcmp(argv[i], "-fiber") == 0) {
			fiber_pathname = argv[++i];
		} else if (strcmp(argv[i], "-dst") == 0) {
			dst_pathname = argv[++i];
		} else if (strcmp(argv[i], "-d") == 0) {
            pos.x = atoi(argv[++i]);
            pos.y = atoi(argv[++i]);
            pos.z = atoi(argv[++i]);
            debug_mode_flag = true;
        } else {
			return -1;
		}
	}

	if (fiber_pathname == NULL || dst_pathname == NULL)
		return -2;

	return 0;
}

void AddFiber(vector<CPoint3I> &map, const int x, const int y, const int z)
{
    for (uint i = 0; i < map.size(); ++i) {
        if (map.at(i).x == x && map.at(i).y == y && map.at(i).z == z) {
            return;
        }
    }

    map.push_back(CPoint3I(x, y, z));
}

void AddFiber(vector<vector<CPoint3I> > &map, const int ix, const int iy, 
        const int w, const int h, const int x, const int y, const int z)
{
    int min_x = ix > 0 ? ix-1 : 0;
    int max_x = ix < (w-1) ? ix+1 : w-1;
    int min_y = iy > 0 ? iy-1 : 0;
    int max_y = iy < (h-1) ? iy+1 : h-1;

    for (int yy = min_y; yy <= max_y; ++yy) {
        for (int xx = min_x; xx <= max_x; ++xx) {
            int id = yy * w + xx;
            
            int flag = 0;
            for (uint i = 0; i < map[id].size(); ++i) {
                if (map[id].at(i).x == x && map[id].at(i).y == y && map[id].at(i).z == z) {
                    flag = 1;
                    break;
                }
            }
            if (flag == 0)
                map[id].push_back(CPoint3I(x, y, z));
        }
    }
}

void MappingFibers(char **confPathname, char **vertPathname, 
        const int w, const int h, const int d, const int scale, 
        const int cz, CFiber **fibers, int *count)
{
    vector<vector<CPoint3I> > map;
    map.resize(w*h);

    for (int z = 0; z < d; ++z) {
        // read fibers
		CFiber *slice;
		int cw, ch;
		ReadFibers(confPathname[z], vertPathname[z], &slice, cw, ch);

        // map fibers to voxels
        for (int y = 0; y < h; ++y) {
            for (int x = 0; x < w; ++x) {
                int index = y * w + x;
                CFiber *f = &(slice[index]);

                // add this fiber to the seed
               // if ((f->m_seed.pos.m_z * scale + 0.5f) >= cz-1 && (f->m_seed.pos.m_z * scale + 0.5f) <= cz+1) {
                if ((f->m_seed.pos.m_z * scale + 0.5f) == cz) {
                    int iy = int(f->m_seed.pos.m_y * scale + 0.5f);
                    int ix = int(f->m_seed.pos.m_x * scale + 0.5f);
                    AddFiber(map[iy*w+ix], x, y, z);
                    //AddFiber(map, ix, iy, w, h, x, y, z);
                }

                // forward
                for (uint k = 1; k < f->m_fCount/2; ++k) {
                    CPoint3F sPt = f->m_pF[k-1];
                    CPoint3F ePt = f->m_pF[k];
                    CVector3F dir = ePt - sPt;
                    
                    for (float l = 0.0f; l < 1.0f; l += 0.1f) {
                        CPoint3F pt = sPt + dir * l;
                        //if (int(pt.m_z * scale + 0.5f) >= cz-1 && int(pt.m_z * scale + 0.5f) <= cz+1) {
                        if (int(pt.m_z * scale + 0.5f) == cz) {
                            int iy = int(pt.m_y * scale + 0.5f);
                            int ix = int(pt.m_x * scale + 0.5f);
                            //AddFiber(map, ix, iy, w, h, x, y, z);
                            AddFiber(map[iy*w+ix], x, y, z);
                        }
                    }
                }

                // backward
                for (uint k = 1; k < f->m_bCount/2; ++k) {
                    CPoint3F sPt = f->m_pB[k-1];
                    CPoint3F ePt = f->m_pB[k];
                    CVector3F dir = ePt - sPt;
                    
                    for (float l = 0.0f; l < 1.0f; l += 0.1f) {
                        CPoint3F pt = sPt + dir * l;
                        //if (int(pt.m_z * scale + 0.5f) >= cz-1 && int(pt.m_z * scale + 0.5f) <= cz+1) {
                        if (int(pt.m_z * scale + 0.5f) == cz) {
                            int iy = int(pt.m_y * scale + 0.5f);
                            int ix = int(pt.m_x * scale + 0.5f);
                            //AddFiber(map, ix, iy, w, h, x, y, z);
                            AddFiber(map[iy*w+ix], x, y, z);
                        }
                    }
                }
                
            }
        }

        delete[] slice;
        
        printf("\rMapping fibers for slice z=%d ... %.2f%% done.", cz, float(z+1)/float(d)*50.0f);
        fflush(stdout);
    }

    if (debug_mode_flag == true) {
        /* output the fibers that go througth that voxel */
        int index = pos.y * w + pos.x;
        count[index] = (int)map[index].size();
        if (count[index] > 0)
            fibers[index] = new CFiber[count[index]];
        else
            fibers[index] = NULL;

        if (count[index] > 0) {
            for (int z = 0; z < d; ++z) {
                // read fibers
                CFiber *slice;
                int cw, ch;
                ReadFibers(confPathname[z], vertPathname[z], &slice, cw, ch);
                
                for (int k = 0; k < count[index]; ++k) {
                    if (map[index][k].z == z) {
                        int id = map[index][k].y * w + map[index][k].x;
                        fibers[index][k].CopyFiber(&(slice[id]));
                    }
                }
                
                delete[] slice;
                
                printf("\rMapping fibers for slice z=%d ... %.2f%% done.", cz, float(z+1)/float(d)*50.0f+50.0f);
                fflush(stdout);
            }

        }

        SaveFibersAsVTK(dst_pathname, fibers[index], count[index]);
    } else {
        int stepCount = 32;
        int stepSize = h / stepCount;
        for (int iStep = 10; iStep < stepCount; ++iStep) {
            printf("%d\n", iStep);
            int *_count = new int[w*stepSize];
            CFiber **_fibers = new CFiber*[w*stepSize];

            for (int i = 0; i < w*stepSize; ++i) {
                int index = i + iStep * stepSize * w;
                _count[i] = (int)map[index].size();
                if (_count[i] > 0) {
                    _fibers[i] = new CFiber[_count[i]];
                } else {
                    _fibers[i] = NULL;
                }
            }

            for (int z = 120; z < 170; ++z) {
                CFiber *slice;
                int cw, ch;
                ReadFibers(confPathname[z], vertPathname[z], &slice, cw, ch);
                
                for (int i = 0; i < w*stepSize; ++i) {
                    int index = i + iStep * stepSize * w;
                    for (int k = 0; k < _count[i]; ++k) {
                        if (map[index][k].z == z) {
                            int id = map[index][k].y * w + map[index][k].x;
                            _fibers[i][k].CopyFiber(&(slice[id]));
                            //printf("%d %d", _fibers[i][k].m_fCount, _fibers[i][k].m_bCount);
                        }
                    }
                }

                delete[] slice;
            }

            // save fibers
            char confPathname[256], vertPathname[256];
            sprintf(confPathname, "%s_%d.conf", dst_pathname, iStep);
            sprintf(vertPathname, "%s_%d.vert", dst_pathname, iStep);
            SaveFibers(confPathname, vertPathname, _fibers, _count, w, stepSize);

            for (int i = 0; i < w*stepSize; ++i) {
                SafeDeleteArray(_fibers[i]);
            }
            SafeDeleteArray(_fibers);
            SafeDeleteArray(_count);
        }
    }


/*    float average = 0.0f;
    float total = 0.0f;
    for (int i = 0; i < w*h; ++i) {
        count[i] = map[i].size();
        if (count[i] > 0)
            fibers[i] = new CFiber[count[i]];
        else
            fibers[i] = NULL;

        if (count[i] > 1) {
            average += count[i];
            total += 1.0f;
        }
    }
    average = average / total;
    printf("average = %f\n", average);

    for (int z = 0; z < d; ++z) {
        // read fibers
		CFiber *slice;
		int cw, ch;
		ReadFibers(confPathname[z], vertPathname[z], &slice, cw, ch);

        for (int i = 0; i < w*h; ++i) {
            for (int k = 0; k < count[i]; ++k) {
                if (map[i][k].z == z) {
                    int id = map[i][k].y * w + map[i][k].x;
                    fibers[i][k].CopyFiber(&(slice[id]));
                }
            }
        }

        delete[] slice;
    }
    */
}

int main(int argc, char *argv[])
{
	if (ParseArguments(argc, argv) != 0) {
		PrintUsage();
		return 0;
	}

	/* read the configure file */
	char **confPathname = NULL;
	char **vertPathname = NULL;
	int w, h, d, scale;
	float stepSize, maxLength;
	ReadConfigureFile(fiber_pathname, &confPathname, &vertPathname, w, h, d, scale, stepSize, maxLength);


    // mapping fibers for each slice
    if (debug_mode_flag == false) {
        for (int z = 144; z < 145; ++z) {
            printf("Mapping fibers for slice z=%d ... 0.0%% done.", z);
            
            CFiber **fibers = new CFiber*[w*h];
            int *count = new int[w*h];
            memset(count, 0, sizeof(int)*w*h);
            memset(fibers, 0, sizeof(CFiber *)*w*h);
            
            // do mapping
            MappingFibers(confPathname, vertPathname, w, h, d, scale, z, fibers, count);
            
            // save
            //SaveFibers();
            
            for (int i = 0; i < w*h; ++i) {
                SafeDeleteArray(fibers[i]);
            }
            delete[] fibers;
            delete[] count;
            
            printf("\rMapping fibers for slice z=%d ... 100.0%% done.\n", z);           
        }
    } else {
        /* for debug */
        for (int z = pos.z; z < pos.z+1; ++z) {
            printf("Mapping fibers for slice z=%d ... ", z);
            
            CFiber **fibers = new CFiber*[w*h];
            int *count = new int[w*h];
            memset(count, 0, sizeof(int)*w*h);
            memset(fibers, 0, sizeof(CFiber *)*w*h);
            
            // do mapping
            MappingFibers(confPathname, vertPathname, w, h, d, scale, z, fibers, count);
            
            // save
            //SaveFibers();
            
            for (int i = 0; i < w*h; ++i) {
                SafeDeleteArray(fibers[i]);
            }
            delete[] fibers;
            delete[] count;
            
            printf("Done.\n");
        }
    }

	for (int z = 0; z < d; ++z) {
		delete[] confPathname[z];
		delete[] vertPathname[z];
	}
	delete[] confPathname;
	delete[] vertPathname;

	return 0;
}
